% 点云数据预处理
% 输入点云数据格式为 N x 3 的矩阵，每行表示一个点的 (r_i, \varphi_i, \theta_i)
% r_i: 距离, \varphi_i: 方位角, \theta_i: 俯仰角
% 输入一个 2D 矩阵，大小为 [num_points, 3]
% 其中每行表示一个点的 (r, \varphi, \theta)
load_data = load('point_cloud_data.mat');
point_cloud = load_data.point_cloud;
[num_points, ~] = size(point_cloud);

% 假设每帧包含固定数量的点，例如 128 个点，计算帧数
% 显示几帧不确定，一个chirp应该不止受到一个点
points_per_frame = 128;
num_frames = floor(num_points / points_per_frame);

% 初始化输出
transformed_points = zeros(num_points, 3);

% 雷达坐标系转换为直角坐标系
% 旋转没写，假设雷达放置是平行地面坐标系的，如有倾角歪斜需加入转换坐标的步骤
for i = 1:num_points
    r_i = point_cloud(i, 1);
    phi_i = point_cloud(i, 2);
    theta_i = point_cloud(i, 3);
    
    % 坐标转换公式 (1)-(3)
    x_i = r_i * cos(theta_i) * sin(phi_i);
    y_i = r_i * cos(theta_i) * cos(phi_i);
    z_i = r_i * sin(theta_i);
    
    transformed_points(i, :) = [x_i, y_i, z_i];
end

% 检测人员活动于 O-xy 平面，z 轴信息忽略，只考虑 x 和 y 坐标
xy_points = transformed_points(:, 1:2);

% 检测区域预提取 
% 这里定义检测区域边界为 (x_min, x_max) 和 (y_min, y_max)
x_min = -4; 
x_max = 4;
y_min = 0;
y_max = 8;

% 过滤掉区域外的点
in_region_idx = (xy_points(:, 1) >= x_min) & (xy_points(:, 1) <= x_max) & ...
                (xy_points(:, 2) >= y_min) & (xy_points(:, 2) <= y_max);
filtered_points = xy_points(in_region_idx, :);
num_filtered_points = size(filtered_points, 1);

% 将过滤后的点按帧数重新分割并生成 gif
filename = 'point_cloud_frames.gif';
figure;
for frame = 1:num_frames
    start_idx = (frame - 1) * points_per_frame + 1;
    end_idx = min(frame * points_per_frame, num_filtered_points);
    if start_idx > num_filtered_points
        break;
    end
    frame_points = filtered_points(start_idx:end_idx, :);
    
    % 可视化结果
    scatter(frame_points(:, 1), frame_points(:, 2), '.');
    title(['检测区域内点云数据 第' num2str(frame) '帧']);
    xlabel('x (m)');
    ylabel('y (m)');
    axis equal;
    drawnow;
    
    % 保存为 gif
    frame_img = getframe(gcf);
    im = frame2im(frame_img);
    [imind, cm] = rgb2ind(im, 256);
    if frame == 1
        imwrite(imind, cm, filename, 'gif', 'Loopcount', inf, 'DelayTime', 0.1);
    else
        imwrite(imind, cm, filename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.1);
    end
end

% 输出过滤后的点云数据
% filtered_points_frames 包含了预处理后位于检测区域内的点云数据，可用于后续分析
filtered_points_frames = transformed_points(in_region_idx, :);
